#include <vector>
#include "robot/robot.h"

Robot::Robot(uint16_t server_port, uint16_t clinet_port) {
    _isRunning = false;
    _re = std::make_shared<Json::Reader>();
    _updServer = std::make_shared<UdpServer>(server_port, clinet_port);
}

void Robot::thread_UDPServer() {
    _updServer->UDPServerStart();
    _updServer->get_udp_server()->onMessageReceived = [&](std::string message, std::string ipv4, uint16_t port) {
        std::unique_lock<std::mutex> lock(_mtx_message);
        // lock.unlock();
        if (!message.empty()) {
            _message = message;
            bool bRet = _re->parse(message, _va);
            bool bCheckCRC = false;
            if (true == bRet) {
                // if (!_va["timestamp"].isNull() && !_va["crc"].isNull()) {
                //     unsigned int timestamp = _va["timestamp"].asUInt();
                //     unsigned int crc = _va["crc"].asUInt();
                //     bCheckCRC = crc == generateCRC(timestamp) ? true: false;
                // }
                
                if (!_va["cmd"].isNull() || !_va["mode"].isNull()) {
                    _port = port;
                    _ipv4 = ipv4;
                    if (_port == 22221) {
                        _port = 22223;
                    }
                    // LOG(INFO) << "[Robot]" << ipv4 << ":" << port << "->" << _message;
                } else if (!_va["upload"].isNull()) {
                    pid_port = port;
                }

                // if (true == bCheckCRC) {
                    _udp_cond.notify_all();
                // }
            }
            message = "";
        }
    };
}

void Robot::thread_op() {
    std::unique_lock<std::mutex> msg_lock(_mtx_message);
    _udp_cond.wait(msg_lock, [this]{return !_va["mode"].isNull();});
    // msg_lock.unlock();

    if (false == _va["mode"].isInt()) {
        LOG(WARNING) << "[Robot] Mode is not Int";
    } else {
        int mode = _va["mode"].asInt();
        launch_mode(mode);        // 启动 ROS Launch 程序
        _last_mode = mode;
        _va.removeMember("mode");
    }
    
    // _va.clear();
    // std::lock_guard<std::mutex> va_lock(_mtx_message);
}

void Robot::thread_UDPSend() {
    std::unique_lock<std::mutex> msg_lock(_mtx_message);
    _udp_cond.wait(msg_lock, [this]{
        bool bRet = !(_va["cmd"].isNull() && _va["upload"].isNull());
        return bRet;
    });
    msg_lock.unlock();

    if (!_va["cmd"].isNull() && pid_port > 0 ) {
        // int cmd = _va["cmd"].asInt();
        // launch_cmd(cmd);
        _updServer->UDPServerSend(pid_ipv4, pid_port, _message);
        _va.removeMember("cmd");
    } else if (!_va["upload"].isNull() && _port > 0) {
        _updServer->UDPServerSend(_ipv4, _port, _message);
        _va.removeMember("upload");
    }

    // if (!(_va["cmd"].isNull() && _va["upload"].isNull())) {
    //     int cmd = _va["cmd"].asInt();
    //     launch_cmd(cmd);

    //     if (!_va["cmd"].isNull() && pid_port > 0 ) {
    //         _updServer->UDPServerSend(pid_ipv4, pid_port, _message);
    //         _va.removeMember("cmd");
    //     } else if (!_va["upload"].isNull() && _port >0) {
    //         LOG(INFO) << "[thread_UDPSend]" << _ipv4 << " " << _port;
    //         _updServer->UDPServerSend(_ipv4, _port, _message);
    //         _va.removeMember("upload");
    //     }
    //     // _va.clear();
    // }
}

// TODO 监控进程
void Robot::launch_mode(const int mode) {
    if (_isRunning == false) {
        if ((mode >= (int)Mode::VelContol && mode <= (int)Mode::Navi)) {
            int idx = mode - 1;
            pid_t pid = fork();
            if (pid < 0) {
                LOG(ERROR) << "[Robot] ROS mode pid start error";
                std::exit(EXIT_FAILURE);
            }
            if (pid == 0) {
                LOG(INFO) << "[Robot] ROS mode start" << mode;
                if (mode == (int)Mode::Navi) {
                    std::string filename = _va["mapname"].asString();
                    std::string cmd = ROS_MODE[idx] + this->get_map_path() + filename;
                    func_cmd(cmd);
                } else {
                    func_cmd(ROS_MODE[idx]);
                }
                std::exit(EXIT_FAILURE);
            }
            _isRunning = true;
        }   
    } else {
        if (mode == (int)Mode::Kill) {
            _isRunning = false;
            stop();
        }
    }
}

void Robot::launch_cmd(const int cmd) {
    if (_last_mode == (int)Mode::CreatMap && cmd == (int)CMD::SaveMap) {
        std::string filename = this->get_map_path() + _va["mapname"].asString();
        std::string cmd = ROS_CMD[0] +  filename;
        std::string mapname = filename + ".pgm";
        func_cmd(cmd);
        usleep(2000);
        while (!isFileExists(mapname)) {
            // sleep(1);
            usleep(500000);  // 500 ms
        }
        curl_image(mapname, filename + ".yaml");
    } 
    
    // else if (_last_mode == 3 && cmd == 4) {
    //     // 初始位姿
    // } else if (_last_mode == 3 && cmd == 5) {
    //     // 目标位姿
    // }
}

inline void Robot::func_cmd(const std::string str) {
    pid_t pid = fork();
    if (pid < 0) {
        LOG(ERROR) << "[Robot] func_cmd pid start error";
        std::exit(EXIT_FAILURE);
    }
    if (pid == 0) {
        LOG(INFO) << "[Robot] func_cmd pid  start";
        std::system(str.c_str());
        std::exit(EXIT_SUCCESS);
    }
}

bool Robot::isRun() {
    return _isRunning;
}

void Robot::stop() {
    std::system("killall -9 roscore");
    std::system("killall -9 rosmaster");
    std::system("killall -9 simple_goal");
    // std::system("rosnode kill -a");
}

void Robot::stop(const std::vector<std::string>& node) {
    for(auto it = node.begin(); it != node.end(); it++) {
        LOG(INFO) << "[Robot] stop cmd " << it->c_str();
        std::system(it->c_str());
	}
    std::system("rosnode cleanup");

    // os.system("rosnode kill $(rosnode list) | grep record")
}